#include "Comum.h"


/**
 * @author Ricardo_Sa
 * @date 2014_03_14
 * @brief pwm_Set_Motor sets the robot's motors
 * @param __IN__ Int direction, Int pulse
 *
 */
void pwm_Set_Motor(INT8U direction, INT16U pulse)
{

	UARTprintf("\nSet Motor: ");

	if(pulse == 0)
	{
		UARTprintf("Motor parado\n");
	}

	// set refresh rate for portb 6
	uint32_t period1 = SysCtlClockGet() / 50; /*Hz*/
	uint8_t extender1 = period1 >> 16;
	period1 &= 0xFFFF;

	pulse = pulse * 200;
	//set duty
	uint32_t period2 = tics_us * pulse;
	uint8_t extender2 = period2 >> 16;
	period2 &= 0xFFFF;

	// Configure output, PB6 as T0CCP0/1 == PWM Main Motor

	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);    //habilita periferico gpio

	ROM_GPIOPinConfigure(GPIO_PB6_T0CCP0);              //configura pino pb6
	ROM_GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_6);  //seta tipo de pino no timer

	GPIOPinConfigure(GPIO_PB7_T0CCP1);
	GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_7);
	// Configure timer
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);   //habilita periferico timer
	TimerConfigure(TIMER0_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM|TIMER_CFG_B_PWM);   //configura timer para pwm
	HWREG(TIMER0_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAPWML|TIMER_CTL_TBPWML);
	//set period
	TimerPrescaleSet(TIMER0_BASE, TIMER_A, extender1);
	TimerLoadSet(TIMER0_BASE, TIMER_A, period1);

	TimerPrescaleSet(TIMER0_BASE, TIMER_B, extender1);
	TimerLoadSet(TIMER0_BASE, TIMER_B, period1);


	//set default value A Initialization duty = 0
	TimerPrescaleMatchSet(TIMER0_BASE, TIMER_A, extender2);
	TimerMatchSet(TIMER0_BASE, TIMER_A, period2);

	TimerPrescaleMatchSet(TIMER0_BASE, TIMER_B, extender2);
		TimerMatchSet(TIMER0_BASE, TIMER_B, period2);

	TimerEnable(TIMER0_BASE, TIMER_A|TIMER_B);


	////////////////////////////////////////////////////////////


	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
	GPIOPinTypeGPIOOutput (GPIO_PORTB_BASE, GPIO_PIN_4);
	GPIOPinTypeGPIOOutput (GPIO_PORTA_BASE, GPIO_PIN_5);

	switch(direction)
	{

	case RIGHT:
		UARTprintf("RIGHT\n");
		GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_4, GPIO_PIN_4);
		GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5,0);
		break;

	case LEFT:
		UARTprintf("LEFT\n");
		GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, GPIO_PIN_5);
		GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_4,0);

		break;

	case FRONT:
		UARTprintf("FRONT\n");
		GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_4,GPIO_PIN_4);
		GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5,GPIO_PIN_5);
		break;

	default:
		GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_4,0);
		GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5,0);

		break;

	}

	DelayTask(100);



}


/**
 * @author Ricardo_Sa
 * @date 2014_03_14
 * @brief pwm_Servo_Set sets pwm for servo
 * @parameters IN duty cicle
 *
 */
void pwm_Servo_Set(INT16U angle_servo)
{
	INT16U pulse;
	UARTprintf("pwm_Servo_Set\n");

    pulse = (((angle_servo*180)/18)+530);

	// set refresh rate for portb 6
	uint32_t period3 = SysCtlClockGet() / 50; /*Hz*/
	uint8_t extender3 = period3 >> 16;
	period3 &= 0xFFFF;

	//set duty
	uint32_t period4 = tics_us * pulse;
	uint8_t extender4 = period4 >> 16;
	period4 &= 0xFFFF;

	// Configure output, PB0 as T2CCP0 == PWM Servo

	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);    //habilita periferico gpio
	ROM_GPIOPinConfigure(GPIO_PB0_T2CCP0);              //configura pino pb0
	ROM_GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_0);  //seta tipo de pino no timer
	// Configure timer
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);   //habilita periferico timer
	TimerConfigure(TIMER2_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM|TIMER_CFG_B_PWM);   //configura timer para pwm
	HWREG(TIMER2_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAPWML|TIMER_CTL_TBPWML);
	//set period
	TimerPrescaleSet(TIMER2_BASE, TIMER_A, extender3);
	TimerLoadSet(TIMER2_BASE, TIMER_A, period3);


	//set default value A Initialization duty = 0
	TimerPrescaleMatchSet(TIMER2_BASE, TIMER_A, extender4);
	TimerMatchSet(TIMER2_BASE, TIMER_A, period4);

	TimerEnable(TIMER2_BASE, TIMER_A);


}
